%% 图5_7~9，5_15~16：可设定电机工作模式、工况和减速比的单关节开环驱动仿真
% 利用循环模拟控制系统时钟，假定每个采样周期都迭代计算电机速度

%% 程序启动，清理内存和绘图
clear;
clc;
clf;

%% 设定电机工作模式、减速比和工况
%% 定义电机工作模式符号
Amp_Voltage = 0;
Amp_Current = 1;

%% 选择电机工作模式
Amp_Type = Amp_Voltage;
% Amp_Type = Amp_Current;

%% 设定减速比
N = 50; 

%% 设定工况
Td_choice = 1 % 水平面内0，竖直面内1

%% 设定控制电压
if (Amp_Type == Amp_Voltage), Uc = [4;8]; end
if (Amp_Type == Amp_Current && N == 10), Uc = [0.88;1.77]; end
if (Amp_Type == Amp_Current && N == 50), Uc = [0.64;1.28]; end

%% 系统和电机参数初始化
m = 0.5;
g = 9.8;
L = 0.1;
Ra = 2.49;
La = 6.1e-4;
Ka = 8.22e-2;
Ke = 8.24e-2;
Br = 4.1e-4;
Ir = 1.19e-5;
Bl = 2e-2;
Il = 5e-3;
Bm = Br + Bl/N^2;
Im = Ir + Il/N^2;

%% 计算电机控制模型参数
%% 速度模式电机模型参数
Ku = 3;
Tmv = (Ra*Im)/(Ke*Ka+Ra*Bm);
Kmv = (Ka*Ku)/(Ke*Ka+Ra*Bm);
Kdv = Kmv * Ra/(Ka*Ku);
%% 力矩模式电机模型参数
Kg = 1;
Kt = Ka*Kg;
Tmt = Im/Bm;
Kmt = Kt/Bm;
Kdt = 1/Bm;
%% 通用电机模型参数
if(Amp_Type == Amp_Voltage) 
    Tm = Tmv;
    Km = Kmv;
    Kd = Kdv;
end
if(Amp_Type == Amp_Current) 
    Tm = Tmt;
    Km = Kmt;
    Kd = Kdt;
end

%% 设置仿真时间、步长和步数
total_time = 3;
if (Td_choice == 0 && Amp_Type == Amp_Voltage), total_time = 0.2; end % 速度模式且水平面工况，总仿真时间
dt = 0.0001; % 步长
time = 0:dt:total_time; % 时间序列
step = total_time / dt + 1; % 步数

%% 初始化仿真过程的中间变量，2维数组对应着两个控制电压
M_theta = [0;0]; % 电机角度
M_omega_pre = [0;0]; % 迭代计算前，当前电机实际转速
M_omega_aft = [0;0]; % 迭代计算后，当前电机实际转速
ia = [0;0]; % 电枢电流
Ue = [0;0]; % 电枢反电动势
Tqm = [0;0]; % 电机控制力矩
Tqd = [0;0]; % 等效到电机转子的扰动力矩
Tqa = [0;0]; % 作用于电机转子的等效力矩
Ud = [0;0]; % 反应干扰力矩影响的放大器输入端等效扰动电压
Uc_d = [0;0]; % 作用于放大器的等效电压
Ua = [0;0]; % 电枢电压

%% 结果保存，用于输出显示
M_omega_draw = zeros(2,step);
ia_draw = zeros(2,step);

%% 主循环
for i = 1:step
    %% 记录电机速度和电流
    M_omega_draw(:,i) = M_omega_pre(:); % 记录当前角速度
    ia_draw(:,i) = ia(:); % 记录当前电流
    
    %% 更新电机角速度
    % 根据电流-力矩关系利用电机动力学模型计算电机输出转速，比电机通用模型更准确     
    if (Amp_Type == Amp_Voltage) % 速度模式电机的电流计算
        Ua = Uc .* Ku;
        ia = (La/Ra)/(La/Ra + dt) .*ia + (dt/Ra)/(La/Ra + dt) .* (Ua - Ue); % 电流迭代
        Ue = Ke .* M_omega_pre; % 该时刻反电动势
    end
    if (Amp_Type == Amp_Current) % 力矩模式电机的电流计算
        ia = Kg .* Uc; % 当前电枢电流
    end
   if (Td_choice == 0), Tqd = [0;0]; end % 水平工况，无扰动力矩
   if (Td_choice == 1), Tqd = (m*g*L/N) .* cos(M_theta/N); end % 竖直工况，当前扰动力矩（重力矩）

    Tqm = Ka .* ia; % 该时刻控制力矩
    Tqa = Tqm - Tqd; % 作用于电机转子的等效力矩
    M_omega_aft = (Im/Bm)/(Im/Bm + dt) .* M_omega_pre + (dt/Bm)/(Im/Bm + dt) .* Tqa; % 根据电流-力矩关系利用电机动力学模型计算电机输出转速，比电机通用模型更准确
    
    M_theta = M_theta + M_omega_pre .* dt; % 更新电机转角
    M_omega_pre = M_omega_aft; % 更新迭代前电机实际转速
    
end

%% 绘制仿真结果
yyaxis left;
p=plot(time,M_omega_draw(1,:),'r-',time,M_omega_draw(2,:),'b-');
p(1).LineWidth=1.5;
p(2).LineWidth=1.5;
% ylabel('\omega_m(t) (rad/s)');
%ylim([0 180]);
set(gca,'ycolor','k');
yyaxis right;
p=plot(time,ia_draw(1,:),'r-.',time,ia_draw(2,:),'b-.');
p(1).LineWidth=1;
p(2).LineWidth=1;
% ylabel('i_a(t) (A)');
set(gca,'ycolor','k');
legend("\omega_m1","\omega_m2","i_a1","i_a2");
% xlabel('t (s)');
